#include "movejoint.h"

#include "ethernet.h"

#define PI 3.1415926

MoveJoint::MoveJoint(QObject *parent)
    : Operation{parent}
    , m_pDescription((Description*)parent)
{
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
    m_dblJointAngles.append(0.0);
}

MoveJoint::~MoveJoint(){

}

void MoveJoint::run(){
    Device* pDevice = m_pDescription->FindDeviceById(m_strDeviceID);
    QList<QVariant> variants;
    for(int i = 0; i < m_dblJointAngles.size(); i++){
        variants.append(QVariant(m_dblJointAngles[i]));
    }
    pDevice->writeData(variants);
}

QList<double>& MoveJoint::dblJointAngles()
{
    return m_dblJointAngles;
}

void MoveJoint::setDblJointAngles(const QList<double> &newDblJointAngles)
{
    m_dblJointAngles = newDblJointAngles;
}

QString MoveJoint::getClassName(){
    return this->metaObject()->className();
}

void MoveJoint::writeXML(QXmlStreamWriter& writer){
    writer.writeStartElement("MoveJoint");
    writer.writeAttribute("device", m_strDeviceID);
    int index = 1;
    foreach(double value, m_dblJointAngles){
        writer.writeTextElement(QString("joint%1").arg(index++), QString::number(value));
    }

    writer.writeEndElement();
}

void MoveJoint::readFromXML(QXmlStreamReader& reader){
    QXmlStreamAttributes attributes = reader.attributes();
    if(attributes.hasAttribute("device")) {
        m_strDeviceID = attributes.value("device").toString();
    }

    m_dblJointAngles.clear();
    QXmlStreamReader::TokenType token = reader.readNext();
    while (!(reader.tokenType() == QXmlStreamReader::EndElement &&
             reader.name() == QStringLiteral("MoveJoint"))) {

        if (reader.name() == QStringLiteral("joint1") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint2") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint3") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint4") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint5") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }else if (reader.name() == QStringLiteral("joint6") && reader.isStartElement()) {
            m_dblJointAngles.append(reader.readElementText().toDouble());
        }

        reader.readNext();
    }
}
